
	#include "ControladoresPID.h"
	
/******************************************************************************
*   						EXTERNAL VARIABLES
******************************************************************************/
	sPID PIDgains;
	unsigned short QuadThrust;
	_iq volatile   SetPitchAngle,SetRollAngle,SetYawAngle;
	
	volatile short PwmCompensationX=-25;
	volatile short PwmCompensationY=-25;
	
/******************************************************************************
*   						LOCAL VARIABLES
******************************************************************************/
	volatile _iq LastProcessValuePitch,LastProcessValueRoll,LastProcessValueYaw;
	_iq PIDAlgorithm(_iq SetPoint,_iq ProcessValue, _iq LastProcessValue, unsigned char Axis);
	
	/* Only for debug porpuses */
	#if DEBUG_PID == 1					
	_iq ErrorPitch,ErrorRoll,ErrorYaw;
	_iq ITermPitch,ITermRoll,ITermYaw;
	_iq PidP,PidR,PidY;
	unsigned short MotorX1,MotorX2,MotorY1,MotorY2;
	unsigned char MotorX1p,MotorX2p,MotorY1p,MotorY2p;
	#endif

/******************************************************************************
*   					GLOBAL FUNCTIONS DEFINITIONS
******************************************************************************/	
/*****************************************************************************
 * Function: 	VfnPIDGainsConfiguration
 * Description: Pass PID gains from Macro to struct variable.
 * Caveats:
 *****************************************************************************/
	void VfnPIDGainsConfiguration(void)				//Function to initiate PID gains
	{
		PIDgains.KpPitch = _IQ(KP_PITCH);
		PIDgains.KiPitch = _IQ(KI_PITCH);
		PIDgains.KdPitch = _IQ(KD_PITCH);
		PIDgains.KpRoll  = _IQ(KP_ROLL);
		PIDgains.KiRoll  = _IQ(KI_ROLL);
		PIDgains.KdRoll  = _IQ(KD_ROLL);
		PIDgains.KpYaw   = _IQ(KP_YAW);
		PIDgains.KiYaw   = _IQ(KI_YAW);
		PIDgains.KdYaw   = _IQ(KD_YAW);
	}

/*****************************************************************************
 * Function: 	VfnPWMModuleConfiguration
 * Description: Configurate PWM Modules, to controll standar Servo signals
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnPWMModuleConfiguration(void)				//Function for PWM modules configuration
	{	
		#if DEVICE == LM3S5749
		SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
		SysCtlPWMClockSet(SYSCTL_PWMDIV_16); 			//PWM Module Frequency = 50Mhz / 16 
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
		
		GPIOPadConfigSet(GPIO_PORTG_BASE, GPIO_PIN_0, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		GPIOPadConfigSet(GPIO_PORTG_BASE, GPIO_PIN_1, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		GPIOPadConfigSet(GPIO_PORTH_BASE, GPIO_PIN_0, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		GPIOPadConfigSet(GPIO_PORTH_BASE, GPIO_PIN_1, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		
		GPIOPinTypePWM(GPIO_PORTG_BASE, GPIO_PIN_0);
		GPIOPinTypePWM(GPIO_PORTG_BASE, GPIO_PIN_1);
		GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_0);
		GPIOPinTypePWM(GPIO_PORTH_BASE, GPIO_PIN_1);
	
		PWMGenConfigure(PWM_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_GEN_SYNC_LOCAL); // configure a PWM generator to left aligned PWM signals for PWM1 and 2(block 0)
		PWMGenConfigure(PWM_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_GEN_SYNC_LOCAL); // configure a PWM generator to left aligned PWM signals for PWM3 and 4(block 1)
		PWMGenPeriodSet(PWM_BASE, PWM_GEN_0, PERIOD_TIME);										// set the period of a PWM generator block 0
		PWMGenPeriodSet(PWM_BASE, PWM_GEN_1, PERIOD_TIME);										// set the period of a PWM generator block 1
		
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_0,BASE_TIME);					// set of Pulse with to 1ms. in all channels
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_1,BASE_TIME);
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_2,BASE_TIME);
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_3,BASE_TIME);
		PWMOutputState(PWM_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT , true); 		//  enable PWM0 and PWM1 outputs
		PWMOutputState(PWM_BASE, PWM_OUT_2_BIT | PWM_OUT_3_BIT , true);			//  enable PWM2 and PWM3 putputs
		PWMSyncUpdate(PWM_BASE,PWM_GEN_0_BIT);
		PWMSyncUpdate(PWM_BASE,PWM_GEN_1_BIT);
		PWMGenEnable(PWM_BASE, PWM_GEN_0); 										// enable the PWM generator block 0 
		PWMGenEnable(PWM_BASE, PWM_GEN_1);										// enable the PWM generator block 1
		PWM_SYNC_R = 0x03;		// Code section necesary to enable propertly the PWM Module on the LM3S5749
		PWM_SYNC_R = 0x00;	    // Reference at LM3S5749 Errata Sheet.
		#endif
		
		#if DEVICE == LM3S3748
		SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM);
		SysCtlPWMClockSet(SYSCTL_PWMDIV_8); 			//PWM Module Frequency = 50Mhz / 16 
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
		
		GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_2, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_3, GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
		
		GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_2);
		GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_3);
	
		PWMGenConfigure(PWM_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_GEN_SYNC_LOCAL); // configure a PWM generator to left aligned PWM signals for PWM3 and 4(block 1)
		PWMGenPeriodSet(PWM_BASE, PWM_GEN_1, PERIOD_TIME);										// set the period of a PWM generator block 1
		
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_2,BASE_TIME);
		PWMPulseWidthSet(PWM_BASE, PWM_OUT_3,BASE_TIME);
		PWMOutputState(PWM_BASE, PWM_OUT_2_BIT | PWM_OUT_3_BIT , true);			//  enable PWM2 and PWM3 putputs
		PWMSyncUpdate(PWM_BASE,PWM_GEN_1_BIT);
		PWMGenEnable(PWM_BASE, PWM_GEN_1);										// enable the PWM generator block 1
		#endif
		
		QuadThrust = BASE_TIME;
	}

/*****************************************************************************
 * Function: 	VfnMainMotorsSpeed
 * Description: Update speed motors
 * Caveats:		
 * Parameters:  Motor Speed
 * Return:		N/A
 *****************************************************************************/	
	void VfnMainMotorsSpeed(short MotorsSpeed)			//Function to setup the main speed in all motors
	{													// MotorSpeed range 0 to 100%
		if(MotorsSpeed > 100)
		{
			MotorsSpeed = 100;	
		}
		if(MotorsSpeed < 0)
		{
			MotorsSpeed = 0;
		}
		QuadThrust = BASE_TIME + (MotorsSpeed*30);
		#if DEVICE == LM3S3748
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_2,(QuadThrust));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_3,(QuadThrust));
		#endif
		
		#if DEVICE == LM3S5749
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_0,(QuadThrust));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_1,(QuadThrust));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_2,(QuadThrust));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_3,(QuadThrust));
		#endif
	}

/*****************************************************************************
 * Function: 	 VfnGetQuadThrust
 * Description:  Return the QuaXcopter Thrust percent.
 * Caveats:		
 * Parameters:  
 * Return:		Thrust "percent"
 *****************************************************************************/	
	unsigned char VfnGetQuadThrust(void)
	{
		return ((QuadThrust) - BASE_TIME)/30;
	}
	
	void VfnSetQuadThrust(unsigned short Thrust)
	{
		QuadThrust = BASE_TIME + (Thrust*30);
	}
	
/*****************************************************************************
 * Function: 	VfnStabilize
 * Description: Run 3 PID controller for each angle.
 * Caveats:		
 * Parameters:  
 * Return:		N/A
 *****************************************************************************/		
	void VfnStabilize(void)
	{
		volatile _iq PIDPitch,PIDRoll,PIDYaw;
		
		PIDPitch = PIDAlgorithm(SetPitchAngle,Pitch,LastProcessValuePitch,PITCH_FLAG);
		PIDRoll  = PIDAlgorithm(SetRollAngle ,Roll ,LastProcessValueRoll,ROLL_FLAG);
		PIDYaw   = PIDAlgorithm(SetYawAngle  ,Yaw  ,LastProcessValueYaw,YAW_FLAG);
		
		#if DEVICE == LM3S3748
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_2,QuadThrust + _IQint(PIDPitch));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_3,QuadThrust - _IQint(PIDPitch));
		#endif
		#if DEVICE == LM3S5749
			MotorX1 = ((QuadThrust) + _IQint(PIDRoll)- PwmCompensationX);
			MotorX2 = ((QuadThrust) - _IQint(PIDRoll)+ PwmCompensationX);
			MotorY1 = ((QuadThrust) + _IQint(PIDPitch));
			MotorY2 = ((QuadThrust) - _IQint(PIDPitch));
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_0, MotorY1);
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, MotorY2);
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, MotorX1);
			PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, MotorX2);
		#endif
		
	}

/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/
/*****************************************************************************
 * Function: 	PIDAlgorithm
 * Description: PID controller core.
 * Caveats:		
 * Parameters:  Set Point, Process Value, Last Process Value, Axis to be control it
 * Return:		PID action.
 *****************************************************************************/	
	_iq PIDAlgorithm(_iq SetPoint,_iq ProcessValue,_iq LastProcessValue, unsigned char Axis)
	{
		volatile _iq Error,PTerm,ITerm,DTerm;
		volatile _iq PID;
		volatile _iq offsetMax,offsetMin;
		//////////////////////////////////////////////////
		//				OFFSET SECTION					//
		//////////////////////////////////////////////////
		offsetMax = (_IQ(OFFSET) + SetPoint);
		offsetMin = (_IQ(-OFFSET) + SetPoint);
		if( (ProcessValue > offsetMin ) && (ProcessValue < offsetMax ) )
		{
			ProcessValue = SetPoint;
		}
		else
		{
			if((ProcessValue < (_IQ(-OFFSET) + SetPoint) ))
			{
				ProcessValue += _IQ(OFFSET);
			}
			else{}
			if((ProcessValue > (_IQ(OFFSET) + SetPoint) ))
			{
				ProcessValue -= _IQ(OFFSET);
			}
			else{}
		}
		
		//////////////////////////////////////////////////
		//				PID ALGORITHM					//
		//////////////////////////////////////////////////
		if (Axis == PITCH_FLAG)
		{
			Error	= ProcessValue - SetPoint;
			PTerm	= _IQmpy(PIDgains.KpPitch , Error);
			ITerm   = _IQmpy(PIDgains.KiPitch ,_IQmpy((Error + LastProcessValuePitch),_IQ(TS_PID)));
			if(ITerm > _IQ(I_TERM_MAX))
			{
				ITerm = _IQ(I_TERM_MAX);
			}
			else{}
			if(ITerm < _IQ(I_TERM_MIN))
			{
				ITerm = _IQ(I_TERM_MIN);
			}
			else{}
			DTerm   = _IQmpy(PIDgains.KdPitch ,_IQdiv((Error - LastProcessValuePitch),_IQ(TS_PID)));
			LastProcessValuePitch = Error;	
			/* Only for debug porpuses */
			#if DEBUG_PID == 1	
				ErrorPitch = Error;
				ITermPitch = ITerm;
			#endif
		}	
		if (Axis == ROLL_FLAG)
		{
			Error	= ((ProcessValue) - SetPoint);
			PTerm	= _IQmpy(PIDgains.KpRoll , Error);
			ITerm   = _IQmpy(PIDgains.KiRoll ,_IQmpy((Error + LastProcessValueRoll),_IQ(TS_PID)));
			if(ITerm > _IQ(I_TERM_MAX))
			{
				ITerm = _IQ(I_TERM_MAX);
			}
			else{}
			if(ITerm < _IQ(I_TERM_MIN))
			{
				ITerm = _IQ(I_TERM_MIN);
			}
			else{}
			DTerm   = _IQmpy(PIDgains.KdRoll ,_IQdiv((Error - LastProcessValueRoll),_IQ(TS_PID)));
			LastProcessValueRoll = Error;	
			#if DEBUG_PID == 1	
				ErrorRoll = Error;
				ITermRoll = ITerm;
			#endif
		}
		if (Axis == YAW_FLAG)
		{
			Error	= ProcessValue - SetPoint;
			PTerm	= _IQmpy(PIDgains.KpYaw , Error);
			ITerm   = _IQmpy(PIDgains.KiYaw ,_IQmpy((Error + LastProcessValueYaw),_IQ(TS_PID)));
			if(ITerm > _IQ(I_TERM_MAX))
			{
				ITerm = _IQ(I_TERM_MAX);
			}
			else{}
			if(ITerm < _IQ(I_TERM_MIN))
			{
				ITerm = _IQ(I_TERM_MIN);
			}
			else{}
			DTerm   = _IQmpy(PIDgains.KdYaw ,_IQdiv((Error - LastProcessValueYaw),_IQ(TS_PID)));
			LastProcessValueYaw = Error;	
			#if DEBUG_PID == 1	
				ErrorYaw = Error;
				ITermYaw = ITerm;
			#endif
		}
		
		PID	= PTerm + ITerm + DTerm;
		PID	= _IQmpy(PID,_IQ(PID_GAIN_STB));
		if(PID > _IQ(PID_MAX))
		{
			PID = _IQ(PID_MAX);
		}
		else{}
		if(PID < _IQ(PID_MIN))
		{
			PID = _IQ(PID_MIN);
		}
		else{}
		/* Only for debug porpuses */
		#if DEBUG_PID == 1	
			if (Axis == PITCH_FLAG)
			{
				PidP = PID;
			}
			if (Axis == ROLL_FLAG)
			{
				PidR = PID;
			}
			if (Axis == YAW_FLAG)
			{
				PidY = PID;
			}
		#endif
		return PID;
	}

	void VfnGetMotorsThrust(unsigned short* MotorThrustp)
	{
		unsigned short BaseTime = BASE_TIME;
		*MotorThrustp = (((MotorX1) - BaseTime) / 31);
		if(*MotorThrustp> (BASE_TIME+BASE_TIME))
		{
			*MotorThrustp=0;
		}
		MotorThrustp++;	
		*MotorThrustp = (((MotorX2) - BaseTime) / 31);
		if(*MotorThrustp> (BASE_TIME+BASE_TIME))
		{
			*MotorThrustp=0;
		}
		MotorThrustp++;
		*MotorThrustp = (((MotorY1) - BaseTime) / 31);
		if(*MotorThrustp> (BASE_TIME+BASE_TIME))
		{
			*MotorThrustp=0;
		}
		MotorThrustp++;
		*MotorThrustp = (((MotorY2) - BaseTime) / 31);
		if(*MotorThrustp> (BASE_TIME+BASE_TIME))
		{
			*MotorThrustp=0;
		}
	}
